/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSource;

/**
 *
 * @author Team 1533
 */
public class PIDTurn {// implements ExtendedPIDOutput {

	boolean enabled = false;
	Jaguar ljag, rjag;
	Encoder lenc, renc;
	Gyro gyro;
	PIDController left, right;
	EncoderSource leftsource, rightsource;
	private double countsL = 0, countsR = 0;
	private double trackWidth;
	private double setpoint = 0;
	boolean isTurningClockwise = false;
	private double leftTicksPerInch = 0, rightTicksPerInch = 0;

	public PIDTurn(double p, double i, double d, double period, Jaguar lJ, Jaguar rJ, Encoder lE, Encoder rE, Gyro g, double trackWidth, double tpiL, double tpiR) {
		ljag = lJ;
		rjag = rJ;
		lenc = lE;
		renc = rE;
		gyro = g;
		leftsource = new EncoderSource(lenc);
		rightsource = new EncoderSource(renc);
		left = new PIDController(p, i, d, leftsource, ljag, period); ////
		right = new PIDController(p, i, d, rightsource, rjag, period); ////
		left.setContinuous();
		right.setContinuous();

		left.setOutputRange(-1, 1);
		right.setOutputRange(-1, 1);

		leftTicksPerInch = tpiL;
		rightTicksPerInch = tpiR;
	}

	//positive is clockwise, angle is in radians
	public void setSetpoint(double angle) {
		double lcounts = lenc.get();
		double rcounts = renc.get();
		if (setpoint < 0) {
			isTurningClockwise = false;
			left.setSetpoint(lcounts + setpoint * trackWidth / leftTicksPerInch);

			right.setSetpoint(0);	//either this or right.setSetpoint(renc.get());
			rightsource.setStationary(true);
		}
		else {
			isTurningClockwise = true;
			right.setSetpoint(rcounts + setpoint * trackWidth / rightTicksPerInch);
			rightsource.setStationary(false);

			left.setSetpoint(0);
			leftsource.setStationary(true);
		}
		rightsource.setStartVal(rcounts);
		leftsource.setStartVal(lcounts);
	}

	public double getSetpoint() {
		return setpoint;
	}

	public boolean isEnabled(){
		return enabled;
	}

	public boolean onTarget(){
		return left.onTarget()&&right.onTarget();
	}

	public double get() {
		if (isTurningClockwise) {
			return left.get();
		}
		else {
			return right.get();
		}
	}

	public void enable() {
		countsL = lenc.get();
		countsR = renc.get();
		left.enable();
		right.enable();
		enabled = true;
	}

	public void reset() {
		countsL = countsR = 0;
		left.reset();
		right.reset();
		enabled = false;
	}

	public void disable() {
		left.disable();
		right.disable();
		enabled = false;
	}

	public void setOutputRange(double min, double max) {
		left.setOutputRange(min, max);
		right.setOutputRange(min, max);
	}

	public void setInputRange(double min, double max) {
		left.setInputRange(min, max);
		right.setInputRange(min, max);
	}

	public void setTolerance(double tol) {
		left.setTolerance(tol);
		right.setTolerance(tol);
	}

	public class EncoderSource implements PIDSource {

		double startVal = 0;
		boolean isStationary;
		Encoder enc;

		public EncoderSource(Encoder e) {
			enc = e;
		}

		public void setStationary(boolean isStationary) {
			this.isStationary = isStationary;
		}

		public void setStartVal(double val) {
			startVal = val;
		}

		public double pidGet() {
			if (isStationary) {
				return enc.get();
			}
			else {
				return enc.get() - startVal;
			}
		}
	}
}
